#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"       // OpenHarmony 初始化接口
#include "cmsis_os2.h"       // CMSIS RTOS 2 接口（任务管理）
#include "iot_gpio.h"        // GPIO 操作接口
#include "hi_io.h"           // 硬件 IO 功能配置接口
#include "hi_time.h"         // 时间相关接口（如 hi_get_tick）
#include "robot_control.h"   // 小车控制自定义接口
#include "hi_adc.h"          // ADC 采样接口
#include "iot_errno.h"       // IoT 错误码定义
#include "hi_pwm.h"          // PWM 模块接口

// 宏定义
#define GPIO5 5                          // 模式切换按键使用的 GPIO 引脚编号
#define FUNC_GPIO 0                      // GPIO 功能模式（非复用功能）
#define ADC_TEST_LENGTH (20)             // ADC 采样次数（多次采样取平均）
#define VLT_MIN (100)                    // 电压最小值阈值（未直接使用，保留）
#define OLED_FALG_ON ((unsigned char)0x01) // OLED 开启标志（未直接使用）
#define OLED_FALG_OFF ((unsigned char)0x00)// OLED 关闭标志（未直接使用）
#define DUTY_70_PERCENT 700              // 70% 占空比
#define DUTY_50_PERCENT 500              // 50% 占空比
#define DUTY_30_PERCENT 300              // 30% 占空比
// 新增 PWM 端口定义，假设使用 PWM0 和 PWM1 控制左右电机
#define PWM_PORT_LEFT HI_PWM_PORT_PWM4
#define PWM_PORT_RIGHT HI_PWM_PORT_PWM1  
#define PWM_FREQ 1000                    // PWM频率1KHz (周期1ms)

// 全局变量（状态与数据存储）
unsigned short  g_adc_buf[ADC_TEST_LENGTH] = { 0 };          // ADC 原始数据缓冲区（未直接使用）
unsigned short  g_gpio5_adc_buf[ADC_TEST_LENGTH] = { 0 };     // GPIO5 对应的 ADC 采样数据缓冲区
unsigned int    g_gpio5_tick = 0;                            // 记录 GPIO5 上次触发中断的时间戳（防抖动）
unsigned char   g_car_control_mode = 0;                      // 小车控制模式（未直接使用）
unsigned char   g_car_speed_control = 0;                     // 小车速度控制（未直接使用）
unsigned int    g_car_control_demo_task_id = 0;              // 小车控制任务 ID（未直接使用）
unsigned char   g_car_status = CAR_STOP_STATUS;              // 小车当前状态（初始为停止）

// 外部函数声明（依赖其他模块实现）
extern float GetDistance(void);          // 获取超声波测距结果（单位：cm）
extern void trace_module(void);          // 寻迹模式控制函数
extern void car_backward(void);          // 小车后退控制
extern void car_forward(void);           // 小车前进控制
extern void car_left(void);              // 小车左转控制
extern void car_right(void);             // 小车右转控制
extern void car_stop(void);              // 小车停止控制
extern void engine_turn_left(void);      // 舵机左转控制
extern void engine_turn_right(void);     // 舵机右转控制
extern void regress_middle(void);        // 舵机归中控制


// 初始化左右电机 PWM
static void pwm_init(void) {
    // 设置 GPIO 为 PWM 输出功能，需要根据实际硬件连接调整 GPIO 引脚
    // 假设左电机对应 GPIO1 为 PWM0 输出
    gpio_control(0, IOT_GPIO_VALUE1);
    hi_io_set_func(1, HI_IO_FUNC_GPIO_1_PWM4_OUT);
    // 假设右电机对应 GPIO10 为 PWM1 输出
    gpio_control(9, IOT_GPIO_VALUE1);
    hi_io_set_func(10, HI_IO_FUNC_GPIO_10_PWM1_OUT);
    // 初始化左右电机的 PWM 端口
    hi_pwm_init(PWM_PORT_LEFT);
    hi_pwm_init(PWM_PORT_RIGHT);
    hi_pwm_set_clock(PWM_CLK_160M);
}

// 以70%占空比速度直线前进
static void car_forward_70_percent(void) {
    pwm_init();
    // 启动左右电机的 PWM，设置占空比为 70%
    hi_pwm_start(PWM_PORT_LEFT, DUTY_70_PERCENT, PWM_FREQ);
    hi_pwm_start(PWM_PORT_RIGHT, DUTY_70_PERCENT, PWM_FREQ);
    // car_forward();
}

// 以50%占空比速度直线前进
static void car_forward_50_percent(void) {
    pwm_init();
    // 启动左右电机的 PWM，设置占空比为 50%
    hi_pwm_start(PWM_PORT_LEFT, DUTY_50_PERCENT, PWM_FREQ);
    hi_pwm_start(PWM_PORT_RIGHT, DUTY_50_PERCENT, PWM_FREQ);
}

// 以30%占空比速度直线前进
static void car_forward_30_percent(void) {
    pwm_init();
    // 启动左右电机的 PWM，设置占空比为 30%
    hi_pwm_start(PWM_PORT_LEFT, DUTY_30_PERCENT, PWM_FREQ);
    hi_pwm_start(PWM_PORT_RIGHT, DUTY_30_PERCENT, PWM_FREQ);
    // car_forward();
}

/**
 * @brief 初始化模式切换按键的 GPIO 配置
 * 
 * 配置 GPIO5 为输入模式，启用上拉电阻，用于检测按键按下事件。
 */
void switch_init(void)
{
    IoTGpioInit(5);                          // 初始化 GPIO5 硬件
    hi_io_set_func(5, 0);                    // 设置 GPIO5 为基础 GPIO 功能（非复用）
    IoTGpioSetDir(5, IOT_GPIO_DIR_IN);       // 设置 GPIO5 为输入方向
    hi_io_set_pull(5, 1);                    // 启用 GPIO5 上拉电阻（按键未按下时为高电平）
}

/**
 * @brief GPIO5 按键中断响应函数（模式切换逻辑）
 * 
 * 检测按键按下事件，通过时间间隔防抖动，切换小车状态（停止→寻迹→避障→70%前进→30%前进→停止循环）。
 */
void gpio5_isr_func_mode(void)
{
    printf("gpio5_isr_func_mode start\n");
    unsigned int tick_interval = 0;          // 两次中断的时间间隔（防抖动）
    unsigned int current_gpio5_tick = 0;     // 当前中断时间戳

    current_gpio5_tick = hi_get_tick();      // 获取当前系统时间（ms）
    tick_interval = current_gpio5_tick - g_gpio5_tick;  // 计算时间间隔

    // 若时间间隔小于保护时间（防抖动），跳过本次中断
    if (tick_interval < KEY_INTERRUPT_PROTECT_TIME) {  
        return;  // 注意：原代码返回 NULL 但函数无返回值，此处修正为 return
    }
    g_gpio5_tick = current_gpio5_tick;       // 更新上次中断时间戳

    // 状态机：循环切换 停止→寻迹→避障→70%前进→30%前进→停止
    switch (g_car_status) {
        case CAR_STOP_STATUS:
            g_car_status = CAR_TRACE_STATUS;
            printf("trace\n");
            break;
        case CAR_TRACE_STATUS:
            g_car_status = CAR_OBSTACLE_AVOIDANCE_STATUS;
            printf("ultrasonic\n");
            break;
        case CAR_OBSTACLE_AVOIDANCE_STATUS:
            g_car_status = CAR_FORWARD_70_PERCENT_STATUS;
            printf("forward 70%%\n");
            break;
        case CAR_FORWARD_70_PERCENT_STATUS:
            g_car_status = CAR_FORWARD_50_PERCENT_STATUS;
            printf("forward 50%%\n");
            break;
        case CAR_FORWARD_50_PERCENT_STATUS:
            g_car_status = CAR_FORWARD_30_PERCENT_STATUS;
            printf("forward 30%%\n");
            break;
        case CAR_FORWARD_30_PERCENT_STATUS:
            g_car_status = CAR_STOP_STATUS;
            printf("stop\n");
            break;
        default:
            g_car_status = CAR_STOP_STATUS;
            break;
    }
}

/**
 * @brief 读取 GPIO5 对应的 ADC 电压值（用于检测按键按下）
 * 
 * 通过 ADC 采样 GPIO5 电压，判断是否触发模式切换逻辑。
 * @param param 未使用（CMSIS 中断回调要求）
 * @return 0 表示执行完成（原代码返回 NULL 但函数返回 unsigned char，此处修正）
 */
unsigned char get_gpio5_voltage(void *param)
{
    int i;
    unsigned short data;                     // ADC 原始采样值
    unsigned int ret;                        // 函数返回值（用于错误判断）
    unsigned short vlt;                      // ADC 采样值临时存储
    float voltage;                           // 转换后的电压值（单位：V）
    float vlt_max = 0;                       // 多次采样中的最大电压
    float vlt_min = VLT_MIN;                 // 多次采样中的最小电压（初始为阈值）

    hi_unref_param(param);                   // 标记参数未使用（避免编译警告）
    memset_s(g_gpio5_adc_buf, sizeof(g_gpio5_adc_buf), 0x0, sizeof(g_gpio5_adc_buf));  // 清空采样缓冲区

    // 多次采样（ADC_TEST_LENGTH 次）
    for (i = 0; i < ADC_TEST_LENGTH; i++) {
        // 读取 ADC 通道 2 数据（4次平均模式，抑制噪声）
        ret = hi_adc_read(HI_ADC_CHANNEL_2, &data, HI_ADC_EQU_MODEL_4, HI_ADC_CUR_BAIS_DEFAULT, 0xF0); 
        if (ret != IOT_SUCCESS) {            // ADC 读取失败
            printf("ADC Read Fail\n");
            return 0;                        // 返回 0 表示错误
        }    
        g_gpio5_adc_buf[i] = data;           // 存储采样值
    }

    // 计算多次采样的电压最大值和最小值
    for (i = 0; i < ADC_TEST_LENGTH; i++) {  
        vlt = g_gpio5_adc_buf[i]; 
        voltage = (float)vlt * 1.8 * 4 / 4096.0;  // ADC 码字转电压（公式：V = (码字值 * 参考电压 * 增益) / 满量程）
        vlt_max = (voltage > vlt_max) ? voltage : vlt_max;  // 更新最大值
        vlt_min = (voltage < vlt_min) ? voltage : vlt_min;  // 更新最小值
    }
    printf("vlt_max is %f\r\n", vlt_max);

    // 若最大电压在 0.6V~1.0V 之间（按键按下时的电压范围），触发模式切换
    if (vlt_max > 0.6 && vlt_max < 1.0) {
        gpio5_isr_func_mode();               // 调用模式切换函数
    } 
    return 0;
}

/**
 * @brief 注册 GPIO5 中断监测（按键事件触发）
 * 
 * 配置 GPIO5 为边沿触发中断（下降沿），绑定中断回调函数 get_gpio5_voltage。
 */
void interrupt_monitor(void)
{
    unsigned int  ret = 0;
    g_gpio5_tick = hi_get_tick();            // 初始化上次中断时间戳

    // 注册 GPIO5 中断：边沿触发（下降沿），回调函数为 get_gpio5_voltage
    ret = IoTGpioRegisterIsrFunc(GPIO5, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, get_gpio5_voltage, NULL);
    if (ret == IOT_SUCCESS) {
        printf(" register gpio5\r\n");       // 中断注册成功提示
    }
}

/**
 * @brief 判断舵机转向方向（避障逻辑核心）
 * 
 * 通过舵机左右转动测量左右两侧障碍物距离，选择距离更远的一侧作为转向方向。
 * @return CAR_TURN_LEFT（左转）或 CAR_TURN_RIGHT（右转）
 */
static unsigned int engine_go_where(void)
{
    float left_distance = 0;                 // 左侧障碍物距离（cm）
    float right_distance = 0;                // 右侧障碍物距离（cm）

    // 舵机左转，测量左侧距离
    engine_turn_left();                      // 舵机左转控制
    hi_sleep(100);                           // 等待舵机到位（100ms）
    left_distance = GetDistance();           // 获取左侧障碍物距离
    hi_sleep(100);                           // 等待测距稳定

    regress_middle();                        // 舵机归中
    hi_sleep(100);                           // 等待舵机到位

    // 舵机右转，测量右侧距离
    engine_turn_right();                     // 舵机右转控制
    hi_sleep(100);                           // 等待舵机到位
    right_distance = GetDistance();          // 获取右侧障碍物距离
    hi_sleep(100);                           // 等待测距稳定

    regress_middle();                        // 舵机归中

    // 比较左右距离，选择更远的一侧
    if (left_distance > right_distance) {
        return CAR_TURN_LEFT;
    } else {
        return CAR_TURN_RIGHT;
    }
}

/**
 * @brief 根据障碍物距离控制小车行动方向（避障逻辑）
 * 
 * 若距离小于阈值（DISTANCE_BETWEEN_CAR_AND_OBSTACLE），则停止、后退并转向；否则继续前进。
 * @param distance 当前前方障碍物距离（cm）
 */
static void car_where_to_go(float distance)
{
    if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {  // 距离过近（小于阈值）
        car_stop();                                      // 停止
        // 停止 PWM 输出
        hi_pwm_stop(PWM_PORT_LEFT);
        hi_pwm_stop(PWM_PORT_RIGHT);
        hi_sleep(500);                                   // 暂停 500ms
        car_backward();                                  // 后退
        hi_sleep(500);                                   // 后退 500ms
        car_stop();                                      // 停止

        unsigned int ret = engine_go_where();            // 获取转向方向
        printf("ret is %d\r\n", ret);

        // 根据转向方向执行左转或右转
        if (ret == CAR_TURN_LEFT) {
            car_left();                                  // 左转
            // 控制左右电机不同速度实现左转
            hi_pwm_start(PWM_PORT_LEFT, DUTY_30_PERCENT, PWM_FREQ);
            hi_pwm_start(PWM_PORT_RIGHT, DUTY_70_PERCENT, PWM_FREQ);
            hi_sleep(500);                               // 左转 500ms
        } else if (ret == CAR_TURN_RIGHT) {
            car_right();                                 // 右转
            // 控制左右电机不同速度实现右转
            hi_pwm_start(PWM_PORT_LEFT, DUTY_70_PERCENT, PWM_FREQ);
            hi_pwm_start(PWM_PORT_RIGHT, DUTY_30_PERCENT, PWM_FREQ);
            hi_sleep(500);                               // 右转 500ms
        }
        car_stop();                                      // 停止（完成转向）
        // 停止 PWM 输出
        hi_pwm_stop(PWM_PORT_LEFT);
        hi_pwm_stop(PWM_PORT_RIGHT);
    } else {
        car_forward();                                   // 距离足够，继续前进
        // 启动左右电机的 PWM，设置占空比为 70%
        hi_pwm_start(PWM_PORT_LEFT, DUTY_70_PERCENT, PWM_FREQ);
        hi_pwm_start(PWM_PORT_RIGHT, DUTY_70_PERCENT, PWM_FREQ);
    } 
}

/**
 * @brief 避障模式主控制函数
 * 
 * 循环监测前方障碍物距离，调用 car_where_to_go 控制小车行动，直到模式切换。
 */
static void car_mode_control_func(void)
{
    float m_distance = 0.0;                                // 当前前方障碍物距离（cm）
    regress_middle();                                      // 舵机归中（初始位置）

    while (1) {
        // 若模式切换（非避障模式），退出循环
        if (g_car_status != CAR_OBSTACLE_AVOIDANCE_STATUS) {
            printf("car_mode_control_func 1 module changed\n");
            regress_middle();                              // 舵机归中（退出前）
            // 停止 PWM 输出
            hi_pwm_stop(PWM_PORT_LEFT);
            hi_pwm_stop(PWM_PORT_RIGHT);
            break;
        }

        m_distance = GetDistance();                        // 获取前方障碍物距离
        car_where_to_go(m_distance);                       // 根据距离控制行动
        hi_sleep(20);                                      // 延时 20ms（降低 CPU 占用）
    }
}

/**
 * @brief 小车控制主任务（状态机核心）
 * 
 * 根据小车当前状态（停止、寻迹、避障、70%前进、30%前进）调用对应控制函数，循环运行。
 * @param param 未使用（CMSIS 任务要求）
 * @return 无返回值（任务永不退出）
 */
void *RobotCarTestTask(void* param)
{
    printf("switch\r\n");
    switch_init();                                         // 初始化模式切换按键 GPIO
    interrupt_monitor();                                   // 注册按键中断监测

    while (1) {
        switch (g_car_status) {                            // 根据当前状态执行不同逻辑
            case CAR_STOP_STATUS:                          // 停止状态
                car_stop();                                // 保持停止
                // 停止 PWM 输出
                hi_pwm_stop(PWM_PORT_LEFT);
                hi_pwm_stop(PWM_PORT_RIGHT);
                break;
            case CAR_OBSTACLE_AVOIDANCE_STATUS:            // 避障状态
                car_mode_control_func();                   // 进入避障控制循环
                break;
            case CAR_TRACE_STATUS:                         // 寻迹状态
                trace_module();                            // 调用寻迹控制函数（外部实现）
                // 停止 PWM 输出
                hi_pwm_stop(PWM_PORT_LEFT);
                hi_pwm_stop(PWM_PORT_RIGHT);
                break;
            case CAR_FORWARD_70_PERCENT_STATUS:            // 70% 前进状态
                car_forward_70_percent();
                break;
            case CAR_FORWARD_50_PERCENT_STATUS:            // 50% 前进状态
                car_forward_50_percent();
                break;
            case CAR_FORWARD_30_PERCENT_STATUS:            // 30% 前进状态
                car_forward_30_percent();
                break;
            default:                                       // 默认状态（无操作）
                // 停止 PWM 输出
                hi_pwm_stop(PWM_PORT_LEFT);
                hi_pwm_stop(PWM_PORT_RIGHT);
                break;
        }
        IoTWatchDogDisable();                              // 禁用看门狗（防止系统复位）
        osDelay(20);                                       // 任务延时 20ms（调度给其他任务）
    }
}

/**
 * @brief 创建小车控制任务（初始化入口）
 * 
 * 配置任务属性并创建 RobotCarTestTask 任务，启动小车控制逻辑。
 */
void RobotCarDemo(void)
{
    osThreadAttr_t attr;                                   // CMSIS 任务属性结构体

    // 配置任务属性
    attr.name = "RobotCarTestTask";                        // 任务名称（用于调试）
    attr.attr_bits = 0U;                                   // 任务属性（无特殊标志）
    attr.cb_mem = NULL;                                    // 任务控制块内存（动态分配）
    attr.cb_size = 0U;                                     // 任务控制块大小（动态分配时忽略）
    attr.stack_mem = NULL;                                 // 任务栈内存（动态分配）
    attr.stack_size = 10240;                               // 任务栈大小（10KB，确保足够运行）
    attr.priority = 25;                                    // 任务优先级（25 为中等优先级）

    // 创建任务，若失败则打印错误信息
    if (osThreadNew(RobotCarTestTask, NULL, &attr) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create RobotCarTestTask!\n");
    }
}

// 注册功能初始化（OpenHarmony 启动时自动调用 RobotCarDemo）
APP_FEATURE_INIT(RobotCarDemo);